/*
 * exploration.h
 *
 *  Created on: Nov 14, 2012
 *      Author: robot
 */

#ifndef EXPLORATION_H_
#define EXPLORATION_H_

#include "ros/ros.h"
#include <math.h> //for M_PI, and fmod.
#include <behavior/behavior.h>
#include <SLAM/Location.h>
#include <sensor/IR.h>
#include <driver/Move.h>

// Define four basic directions, clockwise
#define NORTH  0
#define	 EAST   1
#define	 SOUTH  2
#define WEST   3



class Exploration : public Behavior
{
	struct 	direction {
		bool 	north;
		bool 	east;
		bool 	south;
		bool 	west;
		int 	marked;
	} valid; //contains valid directions that robot can explore


public:
					Exploration			(ros::NodeHandle n);
	bool 			act 				(ros::Publisher publisher);
	void 			distance_callback 	(const sensor::IR::ConstPtr & msg);

private:

	void 				setValid 		(const sensor::IR::ConstPtr & msg);
	void 				turn 			(int dir, driver::Move * move);
	void 		    	chooseMinDist   (driver::Move * move);

	void 				cycleDirection  ();

	ros::NodeHandle 	nodeHandle;
	direction 			grid[20][20]; 
	int 				direction; //actual direction
	int 				x;
	int 				y;
	ros::Subscriber		distance_subscriber;
	ros::ServiceClient 	tracker_client;

	// IR sensor readings.
	float 			frontRight,
					frontLeft,
					leftFront,
					leftBack,
					rightFront,
					rightBack,
					backRight,
					backLeft;

};

#endif /* EXPLORATION_H_ */
